/*
 * Vorbereitung.c
 *
 *  Created on: 14.06.2009
 *      Author: michi
 */
//#include "vorwaertskinematik.h"


#include "../vars.h"


void initialisiereStrukturen() {
	r10.x = 100;
	r10.y = -20;
	r10.z = 30;

	r40.x = -140;
	r40.y = 10;
	r40.z = 30;

	s1r31.x = koerper_1+koerper_2;
	s1r31.y =0.0;
	s1r31.z =0.0;
	s4r64.x= koerper_4+koerper_5;
	s4r64.y=0.0;
	s4r64.z=0.0;

	o.x = 0;
	o.y = 0;
	o.z = 0;



	memcpy(&DrehM01, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM04, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM11s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh01a, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh01b, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh01y, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM44s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh04a, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh04b, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&Dreh04y, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM22s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM05t, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM55s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM05, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM33s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM03, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM66s, &EinheitsMatrix, sizeof(EinheitsMatrix));
	memcpy(&DrehM06, &EinheitsMatrix, sizeof(EinheitsMatrix));

	Dreh01b.m11 = cos(beta_1);
	Dreh01b.m13 = sin(beta_1);
	Dreh01b.m31 = -sin(beta_1);
	Dreh01b.m33 = cos(beta_1);

	Dreh04b.m11 = cos(beta_4);
	Dreh04b.m13 = sin(beta_4);
	Dreh04b.m31 = -sin(beta_4);
	Dreh04b.m33 = cos(beta_4);

	Dreh04y.m11 = cos(gamma_4);
	Dreh04y.m12 = -sin(gamma_4);
	Dreh04y.m21 = sin(gamma_4);
	Dreh04y.m22 = cos(gamma_4);

	DrehM11s.m11 = cos(theta_1);
	DrehM11s.m12 = -sin(theta_1);
	DrehM11s.m21 = sin(theta_1);
	DrehM11s.m22 = cos(theta_1);

	DrehM44s.m11 = cos(theta_4);
	DrehM44s.m12 = -sin(theta_4);
	DrehM44s.m21 = sin(theta_4);
	DrehM44s.m22 = cos(theta_4);

	DrehM01 = multipliziere2Matrizen(Dreh01a, multipliziere2Matrizen(Dreh01b,
			Dreh01y));
	DrehM04 = multipliziere2Matrizen(Dreh04a, multipliziere2Matrizen(Dreh04y,
			Dreh04b));
}
